#ifndef __LIONAV_H_
#define __LIONAV_H_

#include "liomath.h"

#define QUAT_A 0
#define QUAT_B 1
#define QUAT_C 2
#define QUAT_D 3
#define QUAT_R 0
#define QUAT_X 1
#define QUAT_Y 2
#define QUAT_Z 3

#define LLA_LAT 0
#define LLA_LON 1
#define LLA_ALT 2

#define NED_N 0
#define NED_E 1
#define NED_D 2

#define ROT_P 0
#define ROT_Q 1
#define ROT_R 2

#define EULER_ROLL 0
#define EULER_PITCH 1
#define EULER_YAW 2

#define A_EARTH 6378137.0 // Equator Radius In Meters - PRIMARY PARAM
#define F_EARTH 0.0033528106647812047379068011599243 // Flattening - PRIMARY PARAM
#define K_EARTH 0.99664718933521879526209319884008 // k=b/a=1-f
#define B_EARTH 6356752.3142449642 // Pole Radius b=a(1-f)
#define E_EARTH 0.081819190843032727 // Eccentricity  e=sqrt(1-k^2)
#define E2_EARTH 0.00669438 // e^2 = 1-k^2
#define PE_EARTH 40075004.119184121 // Earth Equator Perimeter
#define PP_EARTH 40007849.370732144 // Earth Pole Perimeter
#define PPK_EARTH 6.272656406729 // ppk=pp/a;
#define W_EARTH 7.292115090e-5 // Rotation Of Earth In Radians Per Second
#define G_EARTH 9.8

#define WEF_00 +0.0000000000000000e+000
#define WEF_01 +1.0000000000000000e+000
#define WEF_02 +1.1359084517176959e-015
#define WEF_03 +3.3471899949675493e-003
#define WEF_04 +1.7514852415634330e-014
#define WEF_05 -6.5263247469719669e-004
#define WEF_06 -2.0638120841927807e-013
#define WEF_07 +5.5847100089175774e-005
#define WEF_08 +5.7651106111222816e-013
#define WEF_09 -1.7470102133435337e-006
#define WEF_10 +1.9157092578936385e-012
#define WEF_11 -1.1894332080753976e-007
#define WEF_12 +1.0291304845348275e-012
#define WEF_13 +1.8605346290321801e-008
#define WEF_14 -1.4158019994476538e-013
#define WEF_15 -1.0939784319171272e-009
#define WEF_16 +1.9255647423777056e-014
#define WEF_17 +2.4943710903969171e-011

#define INV_WEF_00 +0.0000000000000000e+000
#define INV_WEF_01 +1.0000000000000000e+000
#define INV_WEF_02 +8.7558541146670610e-015
#define INV_WEF_03 -3.3471899935673447e-003
#define INV_WEF_04 +2.7930262269659778e-013
#define INV_WEF_05 +6.8624349143956830e-004
#define INV_WEF_06 +2.6829580224152494e-012
#define INV_WEF_07 -7.3772821386743070e-005
#define INV_WEF_08 +2.5180690865767019e-011
#define INV_WEF_09 +6.1543501924110622e-006
#define INV_WEF_10 +5.5048465785745293e-011
#define INV_WEF_11 -5.5647834082406256e-007
#define INV_WEF_12 +2.9429736425612418e-011
#define INV_WEF_13 +5.5073388738335893e-008
#define INV_WEF_14 +2.2903345886504667e-012
#define INV_WEF_15 -4.8824292248061951e-009
#define INV_WEF_16 +1.8002960233687304e-014
#define INV_WEF_17 +2.6895039320629088e-010

namespace Lio {

// DCM math
// Rbn = Rotation Body Frame To Navigation (Fixed) Frame
void getNextR(MATRIX3 R, double w[3], double time, MATRIX3 out = NULL);
void EulerToRbn(double roll, double pitch, double yaw, MATRIX3 Rbn);
inline void EulerToRbn(const double euler[3], MATRIX3 Rbn)
{
	EulerToRbn(euler[ROT_P],euler[ROT_Q],euler[ROT_R],Rbn);
}
void GeoToRen(double lat, double lon, MATRIX3 Ren);
void normalR(MATRIX3 R, MATRIX3 out = NULL);
void RbnToEuler(CMATRIX3 Rbn, double euler[3]);
void EulerToQuat(const double euler[3], QUAT q);
void quatToEuler(CQUAT q, double euler[3]);
void quatToRbn(CQUAT q, MATRIX3 Rbn);
void RbnToQuat(CMATRIX3 Rbn, QUAT q);
void getNextQuat(double *q, const double *w, double time, double *out = NULL, BOOL normalize = TRUE);

// Geodesic math
// LLA = Lat,Lon,Alt
// map = X(North),Y(East),Z(alt)
void LLAtoGeoRef(const double* lla, double* map);
void GeoReftoLLA(const double* map, double* lla);
void LLAtoECEF(const double *lla, double *ecef);
void ECEFtoLLA(const double *ecef, double *lla, int iterCount = 1);
// World elliptic integral approximation
inline double wef(double lat)
{
	return	WEF_00+lat*(
			WEF_01+lat*(
			WEF_02+lat*(
			WEF_03+lat*(
			WEF_04+lat*(
			WEF_05+lat*(
			WEF_06+lat*(
			WEF_07+lat*(
			WEF_08+lat*(
			WEF_09+lat*(
			WEF_10+lat*(
			WEF_11+lat*(
			WEF_12+lat*(
			WEF_13+lat*(
			WEF_14+lat*(
			WEF_15+lat*(
			WEF_16+lat*(
			WEF_17
			)))))))))))))))));
}
// World inverse elliptic integral approximation
inline double invwef(double x)
{
	return	INV_WEF_00+x*(
			INV_WEF_01+x*(
			INV_WEF_02+x*(
			INV_WEF_03+x*(
			INV_WEF_04+x*(
			INV_WEF_05+x*(
			INV_WEF_06+x*(
			INV_WEF_07+x*(
			INV_WEF_08+x*(
			INV_WEF_09+x*(
			INV_WEF_10+x*(
			INV_WEF_11+x*(
			INV_WEF_12+x*(
			INV_WEF_13+x*(
			INV_WEF_14+x*(
			INV_WEF_15+x*(
			INV_WEF_16+x*(
			INV_WEF_17
			)))))))))))))))));
}

class INS
{
protected:
	Matrix dXest,Pest;
public:
	enum StateIndice{
		STATE_LAT,
		STATE_LON,
		STATE_ALT,
		STATE_VNX,
		STATE_VNY,
		STATE_VNZ,
		STATE_ROLL,
		STATE_PITCH,
		STATE_YAW,
		STATE_AX,
		STATE_AY,
		STATE_AZ,
		STATE_WP,
		STATE_WQ,
		STATE_WR,
		STATE_SIZE};
	enum CorrectIndice{
		CORRECT_LAT,
		CORRECT_LON,
		CORRECT_ALT,
		CORRECT_SIZE};
	enum InputIndice{
		INPUT_AX,
		INPUT_AY,
		INPUT_AZ,
		INPUT_WP,
		INPUT_WQ,
		INPUT_WR,
		INPUT_SIZE};
	double
			r[3],vn[3],q[4],r_dot[3],
			Rbn[3][3],Rnb[3][3],
			re[3],map[2],mapr[2],
			ab[3],an[3],aInert[3],aCen,vn_dot[3],
			Wnb_b[3],Win_n[3],Win_b[3],Wie_n[3],Wib_b[3],Wen_n[3],
			Rt,Rm,D22,D11,dQ[STATE_SIZE],
			time,dt,beta_a,beta_w;
	BOOL recurs;
	
	Matrix A,dX,Q,H,Ht,R,Z,K,P,Pz,var;
	INS();
	void init(	const double *r0 = NULL, const double *vn0 = NULL, CQUAT q0 = NULL,
				const double *input_accel = NULL, const double *input_gyro = NULL);
	// Set the correction measure - r
	BOOL correct(const double *measure);
	// Upfdate all
	void estimate(double sample, const double *input_accel, const double *input_gyro);
	// Transform rotation
	void transRotation(const double *Wnb_b, double *Wib_b);
	void setStateNoise(const double *dev_state, double beta_accel, double beta_gyro);
	void setMeasureNoise(const Matrix &newR);
};

}; //namespace Lio

#endif // GL_NAVIG_H